/*
 * 控制模块
 * 气垫船循迹控制主体思路如下：
 * （气垫船这里采用串级PID）
 *
 *  首先是转向环，这里转向环作为外环
 *  outer_out = KP_position * position
 *            + KD_position *（position - position_last)
 *  position_last = position
 *  接着是角速度环，这里角速度环是作为内环
 *  gyro_offset = outer_out - gyro_z
 *  output = KP_gyro * gyro_offset
 *         + KD_gyro *(gyro_offset - gyro_offset_last)
 *  gyro_offset_last = gyro_offset
 *  这里将内环的输出作为最终对电机控制的值
 *
 *
 * 控制方案：  首先找到前瞻点的坐标，根据前瞻点，计算出其斜率，其斜率是反馈值，期望为零，然后经行PD运算。
 *         接着，根据位置环的PD运算结果，作为角速度环的期望，陀螺仪Z轴的角速度作为反馈值，最终进行PD运算
 *         得到最终的输出改变量。这里用到串级环的原因是使小车在转向时候更加稳定，不会有超调。
 *         在逐飞车模方案中，小车是有四个控制电机（使得小车在水平面上进行位移）这里我把它分为两类，
 *         首先是侧翼电机，在车体模型中的主要功能是经行位置控制，也就是使反馈中线处于图像的中心部分
 *         其次是后翼电机，在车体模型中的主要功能是进行方向控制，也就是使反馈中线一直保存竖直状态
 *         后翼电机在位置归中时，也有一定作用，侧翼电机对方向控制影响不大
 * */
#include "zf_common_headfile.h"
/*********************************************************参数部分*****************************************************************/

//结构体
pid_param_t PID_speed = {0};                    //速度环参数结构体
pid_param_t PID_position = {0};                 //位置环参数结构体
pid_param_t PID_direction = {0};                //方向环参数结构体
pid_param_t PID_gyro = {0};                     //角速度环参数 结构体

//PD参数
float kp_speed = 1, kd_speed = 0.6;               //速度环参数
float kp_position = 0.46, kd_position = 0.25;     //位置环参数
float kp_direction = 0.6, kd_direction = 0.55;    //方向环参数
float kp_gyro = 0.4,     kd_gyro = 0.2;          //角速度环参数

//坐标点参数
uint8 center_point = 39;                        //中点坐标
uint8 forward_point = 36;                       //前瞻点坐标

//倍率参数
uint8 position_output_multiplier = 20;           //位置输出倍率
uint8 direction_expect_multiplier = 1;          //方向环反馈倍率
uint8 direction_output_multiplier = 2;          //方向输出倍率

//数据参数
int8  side_control_put = 0;                     //侧翼风扇控制外环输出
int8  output_base = 60;                         //基础期望输出
float increment_out = 0;
int8  output = 0;                               //基础输出
int8  direction_put = 0;                        //位置环输出
int8  rear_control_put = 0;                     //后风扇控制外环输出

/*********************************************************侧翼风扇处理*****************************************************************/

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     图像位置环PID控制
// 参数说明     无
// 返回参数     无
// 使用示例     image_pid();
// 备注信息     这里的PID运算是侧边电机的
//-------------------------------------------------------------------------------------------------------------------
float side_position_pid()
{
    float p = pid_pd(&PID_position, center_point - center_line[forward_point]);
    side_control_put = (int8)p;
    return p;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     位置控制
// 参数说明     无
// 返回参数     无
// 使用示例     direction_control();
// 备注信息     位置控制主要是对侧面电机风扇的控制
//-------------------------------------------------------------------------------------------------------------------
void position_control()
{
    side_position_pid();
    if (side_control_put >= 0) {
        duty_sl = 0;
        duty_sr =  position_output_multiplier * side_control_put;

    }
    else {
        duty_sl = -position_output_multiplier * side_control_put;
        duty_sr = 0;
    }

}

/*********************************************************后翼风扇处理*****************************************************************/

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     计算后测电机的偏差
// 参数说明     无
// 返回参数     error（位置环偏差）
// 使用示例     control_pid();
// 备注信息     根据前瞻点的位置来，计算出它附近的斜率作为期望值进行输出
//-------------------------------------------------------------------------------------------------------------------
float error_direction()
{
    float expect;
    expect = center_line[forward_point + 12] - center_line[forward_point - 12];
    return 0 - direction_expect_multiplier * expect;         //期望  - 反馈
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     图像PID控制
// 参数说明     无
// 返回参数     无
// 使用示例     image_pid();
// 备注信息     简单的位置式PID
//         这里用的是前瞻点附近区间的
//-------------------------------------------------------------------------------------------------------------------
float direction_pid()
{
    float p = pid_pd(&PID_direction, error_direction());
    direction_put = (int8)p;
    return p;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     串级PID控制
// 参数说明     无
// 返回参数     无
// 使用示例     control_pid();
// 备注信息     目前不使用
//-------------------------------------------------------------------------------------------------------------------
float control_pid()
{
    float p = pid_pd(&PID_gyro, direction_pid() - imu963ra_gyro_z / 10);//角速度环
    rear_control_put = (int8)p;
    return p;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     速度增量PD控制
// 参数说明     无
// 返回参数     无
// 使用示例     speed_control_pid();
// 备注信息    基础速度环，编码器作为反馈
//-------------------------------------------------------------------------------------------------------------------
void speed_control_pid()
{
    increment_out = increment_pd(&PID_speed, output_base - encoder_data_dir);
    output += (int8)increment_out;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     方向控制
// 参数说明     无
// 返回参数     无
// 使用示例     direction_control();
// 备注信息     方向控制主要是对后面电机风扇的控制
//-------------------------------------------------------------------------------------------------------------------
void direction_control()
{
    control_pid();
    duty_rl = output - direction_output_multiplier * direction_put;
    duty_rr = output + direction_output_multiplier * direction_put;
}

/*********************************************************总控*****************************************************************/
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     pid参数初始化
// 参数说明     无
// 返回参数     无
// 使用示例     pid_param_init();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void pid_param_init()
{
    pid_pd_adjust(&PID_speed, kp_speed, kd_speed);             //速度环参数赋值
    pid_pd_adjust(&PID_position, kp_position, kd_position);       //位置环参数赋值
    pid_pd_adjust(&PID_direction, kp_direction, kd_direction);    //方向环参数赋值
    pid_pd_adjust(&PID_gyro, kp_gyro, kd_gyro);                   //角速度环参数赋值
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     总控制函数
// 参数说明     无
// 返回参数     无
// 使用示例     all_control();
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void all_control()
{
    position_control();               //位置环（主环）
    speed_control_pid();              //速度环
    //direction_control();              //方向环
}
